// HelloKinect.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include "pch.h"
#include "DepthSensor.h"

int KinectDepthSensor::init() {
	uint32_t count = k4a_device_get_installed_count();
	if (count == 0)
	{
		printf("No k4a devices attached!\n");
		return 1;
	}
	
	if (WRAPPER_FAILED(openDevice())) return 1;

	// Get the size of the serial number
	size_t serial_size = 0;
	k4a_device_get_serialnum(device, NULL, &serial_size);

	// Allocate memory for the serial, then acquire it
	char *serial = (char*)(malloc(serial_size));
	k4a_device_get_serialnum(device, serial, &serial_size);
	printf("Opened device: %s\n", serial);
	free(serial);
	return 0;
}

int KinectDepthSensor::openDevice() {
	// Open the first plugged in Kinect device
	device = NULL;
	if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device)))
	{
		printf("Failed to open k4a device!\n");
		return 1;
	}

	deviceOpened = true;
	return 0;
}

int KinectDepthSensor::start() {

	if (transformation != NULL) {
		k4a_transformation_destroy(transformation);
		transformation = NULL;
	}

	configure();

	if (WRAPPER_FAILED(startCamera())) return 1;
	
	if (K4A_FAILED(k4a_device_get_calibration(device, config.depth_mode, config.color_resolution, &calibration))) {
		printf("Failed to get calibration.\n");
		reset();
		return 1;
	}

	colorWidth = calibration.color_camera_calibration.resolution_width;
	colorHeight = calibration.color_camera_calibration.resolution_height;

	transformation = k4a_transformation_create(&calibration);
	if (transformation == NULL) {
		printf("Failed to get tranformation.\n");
		reset();
		return 1;
	}

	return 0;
}

int KinectDepthSensor::configure() {
	// Configure a stream of 4096x3072 BRGA color data at 15 frames per second
	config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
	config.camera_fps = K4A_FRAMES_PER_SECOND_15;
	config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
	// config.color_resolution = K4A_COLOR_RESOLUTION_3072P;
	config.color_resolution = K4A_COLOR_RESOLUTION_720P; 
	// config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
	config.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED;
	config.synchronized_images_only = true; // this seems to be true by default and thus setting is unnecessary
	return 0;
}

int KinectDepthSensor::startCamera() {
	if (cameraStarted) return 0;
	// Start the camera with the given configuration
	if (K4A_FAILED(k4a_device_start_cameras(device, &config))) {
		printf("Failed to start cameras.\n");
		k4a_device_close(device);
		return 1;
	}
	cameraStarted = true;
	return 0;
}

int KinectDepthSensor::update() {
	releaseData();
	// Capture a depth frame
	switch (k4a_device_get_capture(device, &capture, TIMEOUT_IN_MS))
	{
	case K4A_WAIT_RESULT_SUCCEEDED:
		// printf("Got capture.\n");
		captureGot = true;
		return 0;
	case K4A_WAIT_RESULT_TIMEOUT:
		printf("Timed out waiting for a capture\n");
		return 1;
	case K4A_WAIT_RESULT_FAILED:
		printf("Failed to read a capture\n");
		reset();
		return 1;
	}
	return 1;
}

int KinectDepthSensor::getColorFrame(uint8_t** bufferPtr, int* width, int* height) {
	if (colorImageValid) {
		k4a_image_release(colorImage);
		colorImageValid = false;
	}
	colorImage = k4a_capture_get_color_image(capture);
	if (colorImage != NULL) {
		// printf(" | Depth16 res:%4dx%4d\n", k4a_image_get_height_pixels(colorImage), k4a_image_get_width_pixels(colorImage));	
		// int rows = k4a_image_get_height_pixels(colorImage);
		// int cols = k4a_image_get_width_pixels(colorImage);
		colorImageValid = true;
		*bufferPtr = k4a_image_get_buffer(colorImage);
		*width = colorWidth;
		*height = colorHeight;
		return 0;
	}
	else {
		printf("color image is null\n");
		*bufferPtr = NULL;
		*width = *height = 0;
		return 1;
	}
}

int KinectDepthSensor::getDepthFrame(uint8_t** bufferPtr, int* width, int* height) {
	if (depthImageValid) {
		k4a_image_release(depthImage);
		depthImageValid = false;
	}
	
	// Access the depth16 image
	depthImage = k4a_capture_get_depth_image(capture);
#define RGBD
	if (depthImage != NULL) {
#ifdef RGBD
		depthImageValid = true;
		// tranform the depth image to the color camera coordinate
		
		if (rgbdImageValid) {
			k4a_image_release(rgbdImage);
			rgbdImageValid = false;
		}

		if (K4A_FAILED(k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, colorWidth, colorHeight, 0, &rgbdImage))) {
			printf("failed to create rgbd image.\n");
		}

		k4a_transformation_depth_image_to_color_camera(transformation, depthImage, rgbdImage);
		rgbdImageValid = true;
		*bufferPtr = k4a_image_get_buffer(rgbdImage);
		*width = colorWidth;
		*height = colorHeight;
		return 0;
#else
		*bufferPtr = k4a_image_get_buffer(depthImage);
		*height = k4a_image_get_height_pixels(depthImage);
		*width = k4a_image_get_width_pixels(depthImage);
#endif
	}
	else {
		*bufferPtr = NULL;
		*width = *height = 0;
		return 1;
	}
}

int KinectDepthSensor::releaseData() {
	if (captureGot) {
		k4a_capture_release(capture);
		captureGot = false;
	}
	if (rgbdImageValid) {
		k4a_image_release(rgbdImage);
		rgbdImageValid = false;
	}
	if (colorImageValid) {
		k4a_image_release(colorImage);
		colorImageValid = false;
	}
	// Release the image
	if (depthImageValid) {
		k4a_image_release(depthImage);
		depthImageValid = false;
	}
	
	return 0;
}

int KinectDepthSensor::reset() {
	releaseData();

	if (transformation != NULL) {
		k4a_transformation_destroy(transformation);
		transformation = NULL;
	}
	if (cameraStarted)
		k4a_device_stop_cameras(device);
	cameraStarted = false;
	if (deviceOpened)
		k4a_device_close(device);
	deviceOpened = false;
	return 0;
}